#ifndef RTE_KT1260B_IN_ANALOG_H
#define RTE_KT1260B_IN_ANALOG_H

//* @brief 模拟量输入处理类,不含硬件驱动

#include "logger_rte.h"
#include "SimpleKalmanFilter.h"

#ifndef NOISE_FLT_BUFSZ
#define NOISE_FLT_BUFSZ 20
#endif

template<typename analog_type>
class in_analog {
public:
    int poll(uint32_t tick, analog_type in) {
        if (tick < tick_action)
            return -1;
        if (in < _min || in > _max) {
            if (last_error != 1) {
                logger.error("data out of range %d ,%d<->%d", (int) in, (int) _min, (int) _max);
                last_error = 1;
            }
            return -1;
        }
        last_error = 0;
        tick_action = tick + interval;
        _val = klm->updateEstimate(in);
        write(_val);
        _val = noise_flt_ave();
        return 0;
    }

    analog_type value() const {
        return _val;
    }

    int begin(analog_type range_low, analog_type range_high) {
        this->_min = range_low;
        this->_max = range_high;
        depth = NOISE_FLT_BUFSZ;
        data = (analog_type *) malloc(NOISE_FLT_BUFSZ * sizeof(analog_type));
        klm = new SimpleKalmanFilter(0.5, 0.5, 0.1);
        return 0;
    }

    uint32_t interval = 100;
    analog_type _val = 0;
    uint32_t tick_action{};
    SimpleKalmanFilter *klm{};

    void set_factory(u16 i) {
        interval = i;
    }

    analog_type *data;
    uint8_t depth = NOISE_FLT_BUFSZ;
    uint8_t idx = 0;

    void write(analog_type val) {
        data[idx++] = val;
        idx %= depth;
    }

    analog_type noise_flt_ave() {
//        assert(((_max * depth) < (std::numeric_limits<float>::max())));
        float s = 0;
        for (int i = 0; i < depth; i++) {
            s += data[i];
        }
        return (s / depth);
    }

    analog_type _min;
    analog_type _max;
    u8 last_error;
};

#endif // RTE_KT1260B_IN_ANALOG_H
